%===================================================================================================
%[]FUNCTION NAME: SphericalHarmonicsModel.m
%[]AUTHOR: Jinsung Lee
%[]CREATED: 10/02/2017
%[]REVISED: 09/17/2018
%===================================================================================================
%[]FUNCTION DESCRIPTION:
%This function evaluates the equations of motion for the relative two-body problem using a
%fourth-order spherical harmonics model.
%===================================================================================================
%[]INPUT VARIABLE:
%(S)|Current state vector.
%===================================================================================================
%[]OUTPUT VARIABLES:
%(dS)|Current differential state vector
%===================================================================================================
%[]VARIABLE FORMAT:
%(S)|Column Vector {6 x 1}.
%---------------------------------------------------------------------------------------------------
%(dS)|Column Vector {6 x 1}
%===================================================================================================
%[]USER-DEFINED FUNCTIONS:
%None.
%===================================================================================================
%[]COMMENTS:
%None.
%===================================================================================================
function dS = SphericalHarmonicsModel(~,S)
    
    u = 398600.44;
    %[km^3/s^2]Gravitational parameter of the Earth.
    
    Re = 6378.136;
    %[km]Mean equatorial radius of the Earth.
    
    R = S(1:3);
    %[km]Current satellite position WRT the Earth in ECEF coordinates.
    
    V = S(4:6);
    %[km/s]Current satellite velocity WRT the Earth in ECEF coordinates.
    
    r = norm(R);
    %[km]Current satellite distance from the center of the Earth.
    
    x = R(1);
    %[km]Current x-coordinate.
    
    y = R(2);
    %[km]Current y-coordinate.
    
    z = R(3);
    %[km]Current z-coordinate.
    
    J = [0; 1.0826269E-3; -2.5323078E-6; -1.62042999E-6];
    %[]Zonal harmonics.
    
    g0 = -u * R / r^3;
    %[km/s^2]Zeroeth-order acceleration vector due to gravity of the Earth.
    
    g1 = zeros(3,1);
    %[km/s^2]First-order acceleration vector due to gravity of the Earth.
    
    g2 = -u * J(2) * Re^2 / (2 * r^7) * ...
        [ 3 * x * (x^2 + y^2 - 4 * z^2); ...
        3 * y * (x^2 + y^2 - 4 * z^2); ...
        3 * z * (3 * x^2 + 3 * y^2 - 2 * z^2)];
    %[km/s^2]Second-order acceleration vector due to gravity of the Earth.
    
    g3 = -u * J(3) * Re^3 / (2 * r^9) * ...
        [ 5 * x * z * (3 * x^2 + 3 * y^2 - 4 * z^2); ...
        5 * y * z * (3 * x^2 + 3 * y^2 - 4 * z^2); ...
        -3 * x^4 - 3 * y^4 + 24 * y^2 * z^2 - 8 * z^4 - 6 * x^2 * (y^2 - 4 * z^2)];
    %[km/s^2]Third-order acceleration vector due to gravity of the Earth.
    
    g4 = -u * J(4) * Re^4 / (8 * r^11) * ...
        [-15 * x * (x^4 + y^4 - 12 * y^2 * z^2 + 8 * z^4 + 2 * x^2 * (y^2 - 6 * z^2)); ...
        -15 * y * (x^4 + y^4 - 12 * y^2 * z^2 + 8 * z^4 + 2 * x^2 * (y^2 - 6 * z^2)); ...
        -5 * z * (15 * x^4 + 15 * y^4 - 40 * y^2 * z^2 + 8 * z^4 + 10 * x^2 * (3 * y^2 - 4 * z^2))];
    %[km/s^2]Fourth-order acceleration vector due to gravity of the Earth.
    
    dS = zeros(6,1);
    %[]Allocates memory for the differential state vector.
    
    dS(1:3) = V;
    %[km/s]Current velocity WRT the Earth in ECEF coordinates from the rotating frame POV.
    
    dS(4:6) = g0 + g1 + g2 + g3 + g4;
    %[km/s^2]Current acceleration WRT the Earth in ECEF coordinates from the rotating frame POV.
    
end
%===================================================================================================